#include "platform_i2c.h"
#include "i2c_internal.h"
#include "err.h"
#include "os_io.h"


/*
 * I2C expander (connected to I2C0) Port map:
 * Port 0: pins P0-P7
 * P0: DeepSleep Enable
 * P1: SW_reset
 * P02-P04: TP23-TP21
 * P05: User push button
 * P06: User LED 2
 * P07: User LED 1
 *
 * Port 1: Pins P10-P17
 * P10-P17: User DIP switches S1-S8
 *   > Off: Read as 0
 *   > On: Read as 1
 *
 * Command Bytes for register selection:
 * 00: Input port 0    
 * 01: Input port 1
 * 02: Output port 0
 * 03: Output port 1
 * 04: Polarity inversion port 0   //0:same, 1:toggle
 * 05: Polarity inversion port 1
 * 06: Configuration port 0    // 1:input, 0:output
 * 07: Configuration port 1
 */

void dump_i2c_registers(uint32_t port)
{

	printf("ICOAR: 0x%x\n", ICOAR(port));
	printf("ICIMR: 0x%x\n", ICIMR(port));
	printf("ICSTR: 0x%x\n", ICSTR(port));
	printf("ICCLKL: 0x%x\n", ICCLKL(port));
	printf("ICCLKH: 0x%x\n", ICCLKH(port));
	printf("ICCNT: 0x%x\n", ICCNT(port));
	printf("ICDRR: 0x%x\n", ICDRR(port));
	printf("ICSAR: 0x%x\n", ICSAR(port));
	printf("ICDXR: 0x%x\n", ICDXR(port));
	printf("ICMDR: 0x%x\n", ICMDR(port));
	printf("ICIVR: 0x%x\n", ICIVR(port));
	printf("ICEMDR: 0x%x\n", ICEMDR(port));
	printf("ICPSC: 0x%x\n", ICPSC(port));
	printf("ICREVID1: 0x%x\n", ICREVID1(port));
	printf("ICREVID2: 0x%x\n", ICREVID2(port));
	printf("ICPFUNC: 0x%x\n", ICPFUNC(port));
	printf("ICPDIR: 0x%x\n", ICPDIR(port));
	printf("ICPDIN: 0x%x\n", ICPDIN(port));
	printf("ICPDOUT: 0x%x\n", ICPDOUT(port));
	printf("ICPDSET: 0x%x\n", ICPDSET(port));
	printf("ICPDCLR: 0x%x\n", ICPDCLR(port));

}


void i2c_read_reg(uint32_t port, uint8_t reg_addr, uint8_t *value)
{
	uint32_t temp = 0;

	// wait for busy bit condition to get cleared
	temp = 1;
	while (temp) {
		temp = (ICSTR(port) & ICSTR_BUS_BUSY);   // wait for busy bit clear
		temp |= (ICMDR(port) & ICMDR_STOP);  // wait for any pending stop condition clear
	}

	ICSTR(port) |= ICSTR_STOP; // clear stop condition status

	// Set master mode in ICMDR
	ICMDR(port) |= ICMDR_MASTER;

	// Set mode = transmit 
	ICMDR(port) |= ICMDR_TX_MODE;

	// count register to 1
	ICCNT(port) = 1;  // reg-select

	// Set first word in the xmit register
	ICDXR(port) = reg_addr;
	//printf("Initial ICMDR is: 0x%x and ICSTR:0x%x\n", ICMDR(port), ICSTR(port));

	// Start the transaction
	ICMDR(port) |= ICMDR_START;


	// Wait for ARDY bit to trigger a repeat-start
	temp = 0;
	while (!temp) {
		temp = (ICSTR(port) & ICSTR_ARDY);
	}
	//printf("xmit complete for reg-selection during read\n");

	// Restart transaction with read op
	ICMDR(port) &= ~ICMDR_TX_MODE;

	// count = 1
	ICCNT(port) = 1;

	// set NACKMOD to indicate, stop sending more data
	ICMDR(port) |= ICMDR_NACK_MODE;

	// Start the transaction
	ICMDR(port) |= ICMDR_START;
	//printf("Going to start receive now\n");

	// Wait for data
	do {
		temp = (ICSTR(port) & ICSTR_ICRRDY);
	} while (!temp);

	// data arrived
	*value = (uint8_t)(ICDRR(port) & 0xFF);

	// Raise the stop condition
	ICMDR(port) |= ICMDR_STOP;


	return;
}



void i2c_write_reg2(uint32_t port, uint8_t reg_addr, uint8_t *value, uint8_t count)
{
	uint32_t temp = 0;
	uint8_t  i = 0;

	// wait for busy bit condition to get cleared
	temp = 1;
	while (temp) {
		temp = (ICSTR(port) & ICSTR_BUS_BUSY);
		temp |= (ICMDR(port) & ICMDR_STOP);
	}

	//printf("ICSTR: 0x%x, ICMDR:0x%x\n", ICSTR(port), ICMDR(port));
	ICSTR(port) |= ICSTR_STOP;

	// Set master mode in ICMDR
	ICMDR(port) |= ICMDR_MASTER;

	// Set mode = transmit
	ICMDR(port) |= ICMDR_TX_MODE;

	// count register to 2
	ICCNT(port) = count + 1;  // reg-select + reg-write

	// Set first word in the xmit register
	ICDXR(port) = reg_addr;  

	//printf("ICSTR: 0x%x, ICMDR:0x%x\n", ICSTR(port), ICMDR(port));
	// Start the transaction
	ICMDR(port) |= ICMDR_STOP;
	ICMDR(port) |= ICMDR_START;

    i = 0;
	while (i < count) {

		// Wait for xmit ready for next word
		temp = 0;
		while (!temp) {
			temp = (ICSTR(port) & ICSTR_ICXRDY);
		}

		// xmit is ready for next write
		ICDXR(port) = value[i++];  
	}

	return;
}


void i2c_write_reg(uint32_t port, uint8_t reg_addr, uint8_t value)
{
    i2c_write_reg2(port, reg_addr, &value, 1);
    return;
}

#if 0
static void reset_processor(void)
{
    uint8_t value[2] = {0xFF, 0xFD};
	i2c_write_reg2(I2C0, I2CE_OUTPUT_PORT0, value, 2);
	return;
}
#endif

err_t i2c_init(uint32_t port)
{

	uint32_t tmp_value = 0;

	printf("Enabling I2C port. Revision:0x%x-0x%x\n",
			ICREVID1(port), ICREVID2(port));

	//dump_i2c_registers(port);

	// Place the bus in reset
	ICMDR(port) = 0;

	/*
	 * Configure ICMDR mode:
	 * 8 bit data words
	 * Free data mode: disabled
	 * Start byte mode: disabled
	 * Digital loopback: disabled
	 * Repeat mode: disabled
	 * Slave addressing mode: 7 bit
	 * Master mode: Enabled
	 * STP condition: 0
	 * START Condition: 0
	 * free from breakpoint: Run free
	 */
	ICMDR(port) = 0x4400;

	ICSAR(port) = 33;  // check 33 as well
	// Configure pre-scalar clock frequency
	// Final frequency should be b/w 6.7 - 13.3Mhz
	// pre-scalar_freq = input_freq / (prescalar + 1)
	if (port == I2C0) {
		// I2C0 is powered by 24Mhz clock
		// Set prescalar to 1 => freq = 24/(1+1) = 12 (in range)
		ICPSC(port) = 2;
	} else {
		// I2C1 is powered by sysclk/4
		// Needs further calculations in future when using it
		return ERR_NOT_SUPPORTED;
	}


	// @todo: I2C master clock frequency set;
	ICCLKL(port) = 0x2d;
	ICCLKH(port) = 0x2d;

	// Clear interrupts
	ICIMR(port) = 0;  // mask all interrupts

	tmp_value = ICSTR(port);  // interrupt status register
	ICSTR(port) = tmp_value;  // Write 1 to pending interrupts to clear
	while(ICIVR(port));    // Keep reading interrupt vector register to clear all interrupts

	// Bring I2c out of reset
	ICMDR(port) |= ICMDR_IRS;


	//dump_i2c_registers(port);

	/*
	 * Glow the damn LED
	 */
	// Configure P7,P6, P1 as output in config register
	// P7: LED1
	// P6: LED2
	// P1: SW-reset pin
	i2c_write_reg(port, I2CE_OUTPUT_PORT0, 0xC0);
	i2c_write_reg(port, I2CE_CONF_PORT0, 0x3D);
	// Output 0 to pins to glow1

	//reset_processor();



	return ERR_SUCCESS;
}





